/* file name=sg90.c */
#include <stdio.h>
#include <errno.h>
#include <string.h>
#include <stdlib.h>

#include <wiringPi.h>
#include <softPwm.h>

#define RANGE 200     /* 1 means 100 us , 200 means 20 ms 1等于100微妙，200等于20毫秒 */

int move( int pin, int moveAngle )
{
	float degree;
	float angle=(float)moveAngle;
	degree = 5 +  angle / 180.0 * 20.0;
	printf("角度=%f\n",degree);

		softPwmWrite(pin,degree);
        delay(200);
		softPwmWrite(pin,degree);
        // delay(200);
		// softPwmWrite(pin,degree);
		//  softPwmStop(pin);
         softPwmWrite(pin,0);
		 delay(40);

        //  softPwmWrite(pin,0);
        //  softPwmWrite(pin,0);
        //  softPwmWrite(pin,0);

	// printf("%d\n",degree);
	// printf("%d\n",moveAngle);
	// // softPwmStop(pin);
	// // 输入 pwm 后，给 pwm 为 0，防抖
	// softPwmWrite( pin, 0 );
	// softPwmWrite( pin, 0 );

    return 0;
}

int fmain( int num,int moveAngle )
{
	// wpi 引脚 
	// 物理位置 7
	int	pinN_1	= 7;
	// 物理位置 11
	int	pinN_2	= 0;
	// pinMode(pinN_2,OUTPUT);
	// int	moveAngle_1	= 90;
	// int	moveAngle_2	= 90;
	wiringPiSetup();                        /* wiringpi初始化 */
	softPwmCreate( pinN_1, 15, RANGE );     /* 创建一个使舵机转到90的pwm输出信号 */
	// delay(140);
	softPwmWrite( pinN_1, 0 );
	softPwmCreate( pinN_2, 15, RANGE );     /* 创建一个使舵机转到90的pwm输出信号 */
	// delay(140);
	softPwmWrite( pinN_2, 0 );


		if ( !(( moveAngle ) >= 10 && ( moveAngle ) <= 170) ){
			printf( "degree is between 0 and 180\n" );
		}

		if ( num == 0 || num == 1 ){
			if ( num == 0 ){
				moveAngle = moveAngle - 5;
			}
			if ( num == 1 ){
				moveAngle = moveAngle + 5;
			}
			move( pinN_1, moveAngle );
		}

		if ( num == 2 || num == 3 ){
			if ( num == 2 ){
				moveAngle = moveAngle - 3;
			}
			if ( num == 3 ){
				moveAngle = moveAngle + 3;
			}
			move( pinN_2, moveAngle );
		}
    return moveAngle;
}
//编译程序 gcc -Wall -o duoji duoji.c -lwiringPi -lwiringPiDev -lpthread -lrt -lm -lcrypt -shared

//  +------+-----+----------+------+ Model  NanoPC-T4 +------+----------+-----+------+
//  | GPIO | wPi |   Name   | Mode | V | Physical | V | Mode |   Name   | wPi | GPIO |
//  +------+-----+----------+------+---+----++----+---+------+----------+-----+------+
//  |      |     |     3.3V |      |   |  1 || 2  |   |      | 5V       |     |      |
//  |      |     | I2C2_SDA |      |   |  3 || 4  |   |      | 5V       |     |      |
//  |      |     | I2C2_SCL |      |   |  5 || 6  |   |      | GND(0V)  |     |      |
//  |   32 |   7 | GPIO1_A0 |  OUT | 0 |  7 || 8  |   |      | I2C3_SCL |     |      |
//  |      |     |  GND(0V) |      |   |  9 || 10 |   |      | I2C3_SDA |     |      |
//  |   33 |   0 | GPIO1_A1 |  OUT | 0 | 11 || 12 | 1 | IN   | GPIO1_C2 | 1   |  50  |
//  |   35 |   2 | GPIO1_A3 |   IN | 0 | 13 || 14 |   |      | GND(0V)  |     |      |
//  |   36 |   3 | GPIO1_A4 |   IN | 0 | 15 || 16 | 0 | IN   | GPIO1_C6 | 4   |  54  |
//  |      |     |     3.3V |      |   | 17 || 18 | 0 | IN   | GPIO1_C7 | 5   |  55  |
//  |   40 |  12 | GPIO1_B0 |  ALT |   | 19 || 20 |   |      | GND(0V)  |     |      |
//  |   39 |  13 | GPIO1_A7 |  ALT |   | 21 || 22 | 0 | IN   | GPIO1_D0 | 6   |  56  |
//  |   41 |  14 | GPIO1_B1 |  ALT |   | 23 || 24 |   | ALT  | GPIO1_B2 | 10  |  42  |
//  |      |     |  GND(0V) |      |   | 25 || 26 |   | ALT  | GPIO4_C5 | 11  |  140 |
//  |      |     | I2C2_SDA |      |   | 27 || 28 |   |      | I2C2_SCL |     |      |
//  |  132 |  21 | GPIO4_A4 |   IN | 0 | 29 || 30 |   |      | GND(0V)  |     |      |
//  |  133 |  22 | GPIO4_A5 |   IN | 0 | 31 || 32 |   |      | I2S_CLK  |     |      |
//  |  131 |  23 | GPIO4_A3 |   IN | 0 | 33 || 34 |   |      | GND(0V)  |     |      |
//  |  134 |  24 | GPIO4_A6 |   IN | 0 | 35 || 36 | 0 | IN   | GPIO4_A7 | 27  |  135 |
//  |  124 |  25 | GPIO3_D4 |  ALT |   | 37 || 38 |   | ALT  | GPIO3_D5 | 28  |  125 |
//  |      |     |  GND(0V) |      |   | 39 || 40 |   | ALT  | GPIO3_D6 | 29  |  126 |
//  +------+-----+----------+------+---+----++----+---+------+----------+-----+------+